﻿//---------------------------------------------------------------------------

#ifndef RobotH
#define RobotH

#include <vcl.h>
#include "string.h"
#include "Motor.h"

//const float THIGH_Z = 0.3065; // unit: m
//const float KNEE_Z =  0.1302; // unit: m
const float THIGH_Z = 0.2; // unit: m
const float KNEE_Z =  0.06; // unit: m

// David
// HIP_Z		0.1226 m
// THIGH_Z  0.2 m
// KNEE_Z   0.06 m
// ANKLE_Z	0.1226 m

typedef enum {TRADITIONAL = 0, PARALLEL = 1} LegTypeStatus;

typedef enum
{
	YAW 		= 0,
	ROLL 		= 1,
	PITCH 	= 2,
	ROTATE_AXIS_NUM	= 3
} RotateAxisStatus;

typedef enum
{
	SHOULDER	= 0,
	ELBOW			= 1,
	WRIST			= 2,
	HAND_JOINT_NUM	= 3
} HandJointStatus;

typedef enum
{
	HIP				= 0,
	KNEE			= 1,
	ANKLE			= 2,
	LEG_JOINT_NUM		= 3
} LegJointStatus;

// Roll		->		前後軸(Y)
// Yaw		-> 	  左右軸(X)
// Pitch	->		上下軸(Z)
typedef enum
{
	// Configuration according to Kid
	HEAD_YAW							= 0,	// Head: 頭
	HEAD_PITCH						= 1,

	WAIST_PITCH						= 2,	// Waist: 腰
	WAIST_YAW							= 23,

	RIGHT_SHOULDER_YAW 		= 3,	// Right hand: 右手
	RIGHT_SHOULDER_ROLL		= 4,
	RIGHT_SHOULDER_PITCH	= 21,
	RIGHT_WRIST_ROLL			= 5,

	LEFT_SHOULDER_YAW			= 6,	// Left hand: 左手
	LEFT_SHOULDER_ROLL		= 7,
	LEFT_SHOULDER_PITCH		= 22,
	LEFT_WRIST_ROLL				= 8,

	RIGHT_HIP_PITCH				= 9,	// Right hand: 右腳
	RIGHT_HIP_YAW			 		= 10,
	RIGHT_HIP_ROLL				= 11,
	RIGHT_KNEE_YAW				= 12,
	RIGHT_ANKLE_YAW				= 13,
	RIGHT_ANKLE_ROLL			= 14,

	LEFT_HIP_PITCH				= 15,	// Left hand: 左腳
	LEFT_HIP_YAW					= 16,
	LEFT_HIP_ROLL					= 17,
	LEFT_KNEE_YAW					= 18,
	LEFT_ANKLE_YAW				= 19,
	LEFT_ANKLE_ROLL				= 20,

	// Additional Configuration for Adult
	RIGHT_ELBOW_YAW				= 24,
	LEFT_ELBOW_YAW				= 25,

	// Reserve
	HEAD_ROLL							= 26,

	WAIST_ROLL						= 27,

	RIGHT_ELBOW_PITCH			= 28,
  RIGHT_ELBOW_ROLL			= 29,
	RIGHT_WRIST_PITCH			= 30,
	RIGHT_WRIST_YAW				= 31,

	LEFT_ELBOW_PITCH			= 32,
	LEFT_ELBOW_ROLL				= 33,
	LEFT_WRIST_PITCH			= 34,
	LEFT_WRIST_YAW				= 35,

	RIGHT_KNEE_PITCH			= 36,
	RIGHT_KNEE_ROLL				= 37,
	RIGHT_ANKLE_PITCH			= 38,

	LEFT_KNEE_PITCH				= 39,
	LEFT_KNEE_ROLL				= 40,
	LEFT_ANKLE_PITCH			= 41,
	TOTAL_JOINT_NUM       			= 42
} BodyJointStatus;

//typedef struct
//{
//	RotateAxisStatus Axis;
//  BodyPartStatus Part;
//} JointStruct;

//---------------------------------------------------------------------------
class Robot
{
private:	// User declarations

protected:
	int MotorNum;
	int *ID2GB;
	int Joint2GB[TOTAL_JOINT_NUM];
	int Joint2ID[TOTAL_JOINT_NUM];
	int MirrorTable[TOTAL_JOINT_NUM];

public:
	static const int HeadTable[ROTATE_AXIS_NUM];
	static const int WaistTable[ROTATE_AXIS_NUM];
	static const int RightHandTable[HAND_JOINT_NUM][ROTATE_AXIS_NUM];
	static const int RightLegTable[LEG_JOINT_NUM][ROTATE_AXIS_NUM];
	static const int LeftHandTable[HAND_JOINT_NUM][ROTATE_AXIS_NUM];
	static const int LeftLegTable[LEG_JOINT_NUM][ROTATE_AXIS_NUM];

	LegTypeStatus LegType;
	String Name;
	Motor **Robotis;
	bool SyncLR;

	Robot(TWinControl *Sender, int motor_num);
	~Robot();

	int GetMotorNum();

	void SetMirrorTable();
	void SetID2GB();
	void SetJoint2GB();
	void SetJoint2ID();
	BodyJointStatus GetMirrorJoint(BodyJointStatus joint);
	int GetID2GB(int id);
	int GetJoint2GB(BodyJointStatus joint);
	int GetJoint2ID(BodyJointStatus joint);

	bool SetGBColor(int GBid);
	void SetAllGBColor();

	// Component Event
	void __fastcall MotorComponentBar_Change(TObject *Sender);
	void __fastcall MotorComponentButton_Click(TObject *Sender);
	void __fastcall MotorComponentEdit_KeyPress(TObject *Sender, wchar_t &Key);
	void __fastcall MotorComponentCheck_Click(TObject *Sender);

	// Key Combination
	// Knee & Ankle (Yaw)
	void LeftLeg_KA_Up();			//  ≥↑
	void LeftLeg_KA_Down();		//  ≥↓
	void RightLeg_KA_Up(); 		//  ≤↑
	void RightLeg_KA_Down();	//	≤↓
	void BothLeg_KA_Up();			//  ≤↑≥
	void BothLeg_KA_Down();		//  ≤↓≥

	// Hip & Knee (Yaw)
	void LeftLeg_HK_Up();
	void LeftLeg_HK_Down();
	void RightLeg_HK_Up();
	void RightLeg_HK_Down();
	void BothLeg_HK_Up();
	void BothLeg_HK_Down();

	// Hip & Ankle (Roll)
	void LeftLeg_MoveOutward();
	void LeftLeg_MoveInward();
	void RightLeg_MoveOutward();
	void RightLeg_MoveInward();
	void BothLeg_LeanLeft();
	void BothLeg_LeanRight();

  // Combination of KA & HK
	void LeftLeg_Parallel_Up();
	void LeftLeg_Parallel_Down();
	void RightLeg_Parallel_Up();
	void RightLeg_Parallel_Down();
	void BothLeg_Parallel_Up();
	void BothLeg_Parallel_Down();

	// Traditional Leg	->	Hip & Ankle (Yaw)
	// Parallel	Leg			->	Hip & Knee (Yaw)
	void LeftLeg_Parallel_Forward();
	void LeftLeg_Parallel_Backward();
	void RightLeg_Parallel_Forward();
	void RightLeg_Parallel_Backward();

	bool InverseKinematics(float px, float py, float pz, float hip_yaw_angle, int motor_index[5], int* joint_position);
	void Kinematics(int motor_index[5], int joint_position[5], float *position);
};
//---------------------------------------------------------------------------

// Roll		->		前後軸(Y)
// Yaw		-> 	  左右軸(X)
// Pitch	->		上下軸(Z)
const int Robot::HeadTable[ROTATE_AXIS_NUM] =
{
  HEAD_YAW, HEAD_ROLL, HEAD_PITCH
};
//---------------------------------------------------------------------------

const int Robot::WaistTable[ROTATE_AXIS_NUM] =
{
	WAIST_YAW, WAIST_ROLL, WAIST_PITCH
};
//---------------------------------------------------------------------------

const int Robot::RightHandTable[HAND_JOINT_NUM][ROTATE_AXIS_NUM] =
{
	{RIGHT_SHOULDER_YAW, RIGHT_SHOULDER_ROLL, RIGHT_SHOULDER_PITCH},
	{RIGHT_ELBOW_YAW, RIGHT_ELBOW_ROLL, RIGHT_ELBOW_PITCH},
	{RIGHT_WRIST_YAW, RIGHT_WRIST_ROLL, RIGHT_WRIST_PITCH}
};
//---------------------------------------------------------------------------

const int Robot::RightLegTable[LEG_JOINT_NUM][ROTATE_AXIS_NUM] =
{
	{RIGHT_HIP_YAW, RIGHT_HIP_ROLL, RIGHT_HIP_PITCH},
	{RIGHT_KNEE_YAW, RIGHT_KNEE_ROLL, RIGHT_KNEE_PITCH},
	{RIGHT_ANKLE_YAW, RIGHT_ANKLE_ROLL, RIGHT_ANKLE_PITCH}
};
//---------------------------------------------------------------------------

const int Robot::LeftHandTable[HAND_JOINT_NUM][ROTATE_AXIS_NUM] =
{
	{LEFT_SHOULDER_YAW, LEFT_SHOULDER_ROLL, LEFT_SHOULDER_PITCH},
	{LEFT_ELBOW_YAW, LEFT_ELBOW_ROLL, LEFT_ELBOW_PITCH},
	{LEFT_WRIST_YAW, LEFT_WRIST_ROLL, LEFT_WRIST_PITCH}
};
//---------------------------------------------------------------------------

const int Robot::LeftLegTable[LEG_JOINT_NUM][ROTATE_AXIS_NUM] =
{
	{LEFT_HIP_YAW, LEFT_HIP_ROLL, LEFT_HIP_PITCH},
	{LEFT_KNEE_YAW, LEFT_KNEE_ROLL, LEFT_KNEE_PITCH},
	{LEFT_ANKLE_YAW, LEFT_ANKLE_ROLL, LEFT_ANKLE_PITCH}
};
//---------------------------------------------------------------------------

typedef enum
{
	/*	Construct
		The Robot Class is used to edit individual pose of the moion.

		When the Robot object, which will be named as "Humanoid" in FormPose,
	is constructed [1], every member of the Robot will be set as default value.

		Hence, the members of the Robot class should be assigned the correct value
	in the constructor of FormPose [2], or read config file [3] before using the Robot
	object.
	////////////////////////////////////////////////////////////////////////////
	Associated function:
	1.	Robot::Robot
	2.	TFormPose::FormCreate
	3.	TFormPose::ButtonOpenClick
	////////////////////////////////////////////////////////////////////////////
	*/

	/*	Load Selected Pose from FormMotion
		In order to use the Component of the Motor, such as ScrollBar, Edit and
	Button, to adjust the position of the Motor. We use the event function [1]
	to change the position of the Motor.

		Once "Button_edit" in FormMotion is clicked [2], the position of the selected
	column will be load to Humanoid::Robotis[i]::Position [3]. However, Position of
	Motor is not accessible, since it is a private member of Motor. Therefore,
	Position should be set by using the OnKeyPress event of MEdit or the OnChange
	event of MBar. Besides, InuputPosition of Motor is also set as the position
	of the selected	column in this time.
	////////////////////////////////////////////////////////////////////////////
	Associated function:
	1.	Robot::MotorComponentBar_Change
			Robot::MotorComponentButton_Click
			Robot::MotorComponentEdit_KeyPress
			Robot::MotorComponentCheck_Click
	2.	TFormMotion::Button_editClick
	3.	TFormPose::Button_checkClick
			TFormPose::Button_allClick
	////////////////////////////////////////////////////////////////////////////
	*/

	/*	Send Position of Motor to Control Board
		In FormPose, Timer1 [1] is set to add send data, i.e. OutputData of Motor,
	into send_PC, when Position or Torque of any Motor is changed. When send_PC
	is not empty, the data of send_PC is send [2] though Zigbee by Timer_send [3].
		On the other hand, as send_PC is empty, HMI will send 5 0xAA to control board
	regularly by Timer_rd [4].

	Flow chart:
		Change Position of Motor
							↓
		Trigger MotorComponent Event
							↓
		Set OutputData of Motor
							↓
		Add send job to send_PC										←      Timer1
							↓
		Send to Control Board through Zigbee			←      Timer_Send
							↓
		send_PC is empty
							↓
		Send 5 0xAA through Zigbee                ←      Timer_rd
	////////////////////////////////////////////////////////////////////////////
	Associated function:
	1.	TFormPose::Timer1Timer
	2.	TFormPose::send
	3.	TFormPose::Timer_sendTimer
	4.	TFormPose::Timer_rdTimer
	////////////////////////////////////////////////////////////////////////////
	*/

	/*
		When Pose editing is finished, Positions of Motors should be saved
	to TFormMotion::table_p [1].

		When Motion editing is finished and TFormMotion::Button_play is clicked [2]
	, each pose of Motion is added into send_PC by send_n[3] in FormMotion.
		////////////////////////////////////////////////////////////////////////////
	Associated function:
	1.	TFormPose::Button_okClick
	2.  TFormMotion::Button_playClick
	3.  TFormMotion: send_n
	////////////////////////////////////////////////////////////////////////////
	*/
} CommentOfClass_Robot;



//---------------------------------------------------------------------------

#endif
